#ifdef KITSCHY_DEBUG_MEMORY 
#include "debug_memorymanager.h"
#endif

#ifdef WIN32
#include "windows.h"
#endif

#include "stdio.h"
#include "stdlib.h"
#include "string.h"
#include "math.h"

#include <GL/gl.h>
#include "GL/glu.h"
#include <GL/glut.h>

#include "BList.h"
#include "BVector.h"
#include "geometrics.h"
#include "quaternion.h"
#include "symbol.h"
#include "sort.h"
#include "ontology.h"
#include "featureterm.h"
#include "FTKBase.h"

#include "CMC.h"
#include "3dobject.h"
#include "AWLocalViewport.h"

#include "ode/ode.h"

#include "AWEntity.h"
#include "AWMessage.h"
#include "AWScenario.h"
#include "AWPhysicalScenario.h"
#include "AWBody.h"
#include "AWPhysicalBody.h"
#include "AWSensor.h"
#include "AWBrain.h"
#include "AWAgent.h"
#include "AWWorld.h"
#include "AWPhysicalWorld.h"

#include "loader.h"


#define MAX_CONTACTS 4

// this is called by dSpaceCollide when two objects in space are
// potentially colliding.

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  int i;
  AW_PhysicalWorld *world=(AW_PhysicalWorld *)data;
  // if (o1->body && o2->body) return;

  // exit without doing anything if the two bodies are connected by a joint
  dBodyID b1 = dGeomGetBody(o1);
  dBodyID b2 = dGeomGetBody(o2);
//  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;

  dContact contact[MAX_CONTACTS];   // up to MAX_CONTACTS contacts per box-box
  for (i=0; i<MAX_CONTACTS; i++) {
    contact[i].surface.mode = dContactBounce|dContactApprox1_1; 
//    contact[i].surface.mu = dInfinity;
    contact[i].surface.mu = 0.5;
    contact[i].surface.mu2 = 0;
    contact[i].surface.bounce = 0.3;
    contact[i].surface.bounce_vel = 0.1;
  }
  if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
			   sizeof(dContact))) {
    for (i=0; i<numc; i++) {
      dJointID c = dJointCreateContact (world->get_world_ID(),world->get_contactgroup(),contact+i);
      dJointAttach (c,b1,b2);
    }
  }
}


AW_PhysicalWorld::AW_PhysicalWorld() : AW_World() 
{
	class_name->set("AW_PhysicalWorld");

	{
		List<AW_Agent> l;
		AW_Agent *a;

		ode_world_ID=dWorldCreate();
		space=dHashSpaceCreate(0);
		contactgroup=dJointGroupCreate(0);
		dWorldSetGravity(ode_world_ID,0,9.81F,0);
		dWorldSetCFM (ode_world_ID,1e-5);

		if (scn!=0 && scn->is_a("AW_PhysicalScenario")) {
			((AW_PhysicalScenario *)scn)->set_ode_world(ode_world_ID,space);
		} /* if */ 

		l.Instance(Agents);
		l.Rewind();
		while(l.Iterate(a)) {
			if (a->get_Body()->is_a("AW_PhysicalBody")) {
				((AW_PhysicalBody *)a->get_Body())->set_ode_world(ode_world_ID,space);
			} /* if */ 
		} /* while */ 
	}
} /* AW_CollisionWorld */ 


AW_PhysicalWorld::AW_PhysicalWorld(FILE *fp)
{
	class_name->set("AW_PhysicalWorld");
	load(fp);

	{
		List<AW_Agent> l;
		AW_Agent *a;

		ode_world_ID=dWorldCreate();
		space=dHashSpaceCreate(0);
		contactgroup=dJointGroupCreate(0);
		dWorldSetGravity(ode_world_ID,0,9.81F,0);
		dWorldSetCFM (ode_world_ID,1e-5);

		if (scn!=0 && scn->is_a("AW_PhysicalScenario")) {
			((AW_PhysicalScenario *)scn)->set_ode_world(ode_world_ID,space);
		} /* if */ 

		l.Instance(Agents);
		l.Rewind();
		while(l.Iterate(a)) {
			if (a->get_Body()->is_a("AW_PhysicalBody")) {
				((AW_PhysicalBody *)a->get_Body())->set_ode_world(ode_world_ID,space);
			} /* if */ 
		} /* while */ 
	}
} /* AW_PhysicalWorld::AW_PhysicalWorld */ 


AW_PhysicalWorld::~AW_PhysicalWorld() 
{
	Agents.Delete();
	delete scn;
	scn=0;

	dJointGroupDestroy(contactgroup);
	dSpaceDestroy (space);
	dWorldDestroy(ode_world_ID);
} /* AW_CollisionWorld */ 


void AW_PhysicalWorld::actualize(float time,float timeinc)
{
	AW_Agent *a;
	/* Actualizar el mundo: */ 
	act_time=time;

	actualizeagentlist();
	{
		List<AW_Agent> l;
		AW_Agent *a;

		l.Instance(Agents);
		l.Rewind();
		while(l.Iterate(a)) {
			if (a->get_Body()->is_a("AW_PhysicalBody")) {
				((AW_PhysicalBody *)a->get_Body())->set_ode_world(ode_world_ID,space);
			} /* if */ 
		} /* while */ 
	}

	if (timeinc>0) {
		dSpaceCollide(space,(void *)this,&nearCallback);
//		dWorldStep(ode_world_ID,timeinc);
		dWorldStepFast1(ode_world_ID,timeinc,10);
		dJointGroupEmpty(contactgroup);
	} /* if */ 

	scn->actualize(time,timeinc);

	/* Actualizar los agentes: */ 
	Agents.Rewind();
	while(Agents.Iterate(a)) a->actualize(time,timeinc,this);

	actualizeagentlist();
} /* AW_PhysicalWorld::actualize */ 



bool AW_PhysicalWorld::is_a(char *c)
{
	if (strcmp(c,"AW_PhysicalWorld")==0) return true;

	return AW_World::is_a(c);
} /* AW_PhysicalWorld::is_a */ 


bool AW_PhysicalWorld::is_a(Symbol *c)
{
	if (c->cmp("AW_PhysicalWorld")) return true;

	return AW_World::is_a(c);
} /* AW_PhysicalWorld::is_a */ 


